import rospy
import os
import yaml
import time
import threading
from threading import Thread
import queue
import module.IMU_Optimize
from sensor_msgs.msg import Imu

class IMU_Synchronizer(Thread):
    def __init__(self,imuTopic,imuPub,imuRate):
        super(IMU_Synchronizer,self).__init__()
        self.isAlive=True
        self.imuBuffer=queue.Queue()
        self.IMU_Processed=queue.Queue()
        self.processIMU=None
        self.publishIMU=None
        self.imuTemplate=[]
        self.imurate=rospy.Rate(200)
        self.imusub = rospy.Subscriber(imuTopic, Imu,self.handleIMU, queue_size=200)
        self.imupub=rospy.Publisher(imuPub,Imu,queue_size=500)
        self.lastUpdate=0

    def handleIMU(self,data):
        self.putIMU(data)

    def run(self):
        self.processIMU = threading.Thread(target=self.producer)
        self.publishIMU = threading.Thread(target=self.consumer)
        self.processIMU.start()
        time.sleep(30)
        self.publishIMU.start()

    def producer(self):
        while True:
            list = []
            Max=0
            current = time.perf_counter()
            if self.imuBuffer.qsize()>300:
                Max=300
            else:
                if self.lastUpdate==0 or (self.lastUpdate!=0 and  current-self.lastUpdate<9):
                    print("Not enough IMU,waiting for new data")
                    time.sleep(3)
                    continue
                else:
                    print("too long for new data")
                    if self.imuBuffer.qsize()==0:
                        print("No imu received!End this thread")
                        self.isAlive=False
                        return
                    else:
                        Max=self.imuBuffer.qsize()
            for i in range(0, Max):
                list.append(self.imuBuffer.get())
            list.reverse()
            if len(self.imuTemplate) == 0:
                self.imuTemplate = list.copy()
                while len(list) > 0:
                    self.IMU_Processed.put(list.pop())
            else:
                if abs(len(list) - len(self.imuTemplate)) > 100:
                    self.imuTemplate = []
                    while len(list) > 0:
                        self.IMU_Processed.put(list.pop())
                    self.lastUpdate = time.perf_counter()
                    continue
                score = module.IMU_Optimize.estimate_dtw(list, self.imuTemplate)
                if score > 0.7:
                    print("DTW Not matching!")
                    self.dataReplace(list, self.imuTemplate)
                else:
                    while len(list) > 0:
                        self.IMU_Processed.put(list.pop())
            self.lastUpdate = time.perf_counter()

    def consumer(self):
        while True:
            if self.IMU_Processed.qsize() > 0:
                item = self.IMU_Processed.get()
                self.imupub.publish(item)
                self.imurate.sleep()
            else:
                if self.isAlive:
                    time.sleep(1)
                else:
                    print("IMU Producer stopped , end this thread too")
                    return

    def dataReplace(self,list,template):
        print("data replaceing")

    def putIMU(self,data):
        self.imuBuffer.put(data)

    def getIMU(self):
        return self.imuBuffer.get()

